package com.gitee.wsl.mathematics.function

import com.gitee.wsl.ext.number.lerp


fun bezierInterpolate(start: Double, control1: Double, control2: Double, end: Double, t: Double): Double {
    /* Formula from Wikipedia article on Bezier curves. */
    val omt: Double = 1.0 - t
    val omt2 = omt * omt
    val omt3 = omt2 * omt
    val t2: Double = t * t
    val t3: Double = t2 * t

    return start * omt3 + control1 * omt2 * t * 3.0 + control2 * omt * t2 * 3.0 + end * t3
}

fun cubicInterpolate(
    from: Double,
    to: Double,
    pre: Double,
    post: Double,
    weight: Double
) = 0.5f *
        ((from * 2.0f) +
                (-pre + to) * weight +
                (2.0f * pre - 5.0f * from + 4.0f * to - post) * (weight * weight) +
                (-pre + 3.0f * from - 3.0f * to + post) * (weight * weight * weight))


fun cubicInterpolateInTime(
    from: Double,
    to: Double,
    pre: Double,
    post: Double,
    weight: Double,
    toT: Double,
    preT: Double,
    postT: Double
): Double {
    val t = lerp(0.0, toT, weight)
    val a1 = lerp(pre, from, if (preT == 0.0) 0.0 else (t - preT) / -preT)
    val a2 = lerp(from, to, if (toT == 0.0) 0.5 else t / toT)
    val a3 = lerp(to, post, if (postT - toT == 0.0) 1.0 else (t - toT) / (postT - toT))
    val b1 = lerp(a1, a2, if (toT - preT == 0.0) 0.0 else (t - preT) / (toT - preT))
    val b2 = lerp(a2, a3, if (postT == 0.0) 1.0 else t / postT)
    return lerp(b1, b2, if (toT == 0.0) 0.5 else t / toT)
}
